#ifndef APPLICATION_HPP
#define APPLICATION_HPP

#include <iostream>
#include <string>
#include <thread>
#include <vector>
#include <future>
#include <mutex>
#include <map>
#include <condition_variable>
#include <termios.h>
#include <unistd.h>
#include <signal.h>
#include <termios.h>
#include <mqueue.h>
#include <functional>
#include <mqtt_client.hpp>
#include <unitree/robot/g1/loco/g1_loco_client.hpp>
#include <unitree/robot/g1/arm/g1_arm_action_client.hpp>
#include <unitree/robot/g1/audio/g1_audio_client.hpp>
#include <radar.hpp>
#include <kalman.hpp>
#include <coordinates.hpp>
#include <audioplay.hpp>
#include <nlohmann/json.hpp>

using BitmaskType = uint32_t;
const int MAX_FLAGS = sizeof(BitmaskType) * 8; 

const int MOVE_DURATION = 3;    // seconds
const float MOVE_VELOCITY = 0.2; 

enum class FlagID : int{
    MASK_GAIT = 0,
};


class Application {
public:
    Application(int argc, char *argv[]);
    ~Application();

    int run();

    void setFlag(FlagID flag, bool value = true) {
        int bit_index = static_cast<int>(flag);
        checkBitIndex(bit_index);
        if (value) {
            Mask_ |= (static_cast<BitmaskType>(1) << bit_index);
        }
        else {
            Mask_ &= ~(static_cast<BitmaskType>(1) << bit_index);
        }
    }

    bool getFlag(FlagID flag) const {
        int bit_index = static_cast<int>(flag);
        checkBitIndex(bit_index);
        return (Mask_ & (static_cast<BitmaskType>(1) << bit_index)) != 0;
    }

    void clearFlag(FlagID flag) {
        setFlag(flag, false);
    }
private:
    void Initialize();
    void Loop();
    void POSIX_Message();
    void MQTT_Message();

    void Schedule(std::function<void()> func);

    void Parse_Json(std::string str);
    bool executeAndWaitForState(const std::string& target_state_str, std::function<void()> action_func, int timeout_ms = 20000);

    void checkBitIndex(int bit_index) const {
        if (bit_index < 0 || bit_index >= MAX_FLAGS) {
            throw std::out_of_range("FlagID bit index out of range.");
        }
    }

    radar *radar_;
    std::unique_ptr<unitree::robot::g1::LocoClient> roclient_;
    std::unique_ptr<unitree::robot::g1::G1ArmActionClient> armclient_;
    std::unique_ptr<mqtt::async_client> mqttclient_;
    mqtt::connect_options options_;
    std::unique_ptr<callback> mqttcb_;
    Coordinates coordinates_;
    Audioplay audio_;
    mqd_t mq_;
    struct mq_attr attr_;

    std::mutex mutex_;
    std::condition_variable cv_;
    std::vector<char> msg_buffer;
    std::list<std::function<void()>> main_task_;
    std::vector<std::future<void>> thread_;
    uint32_t Mask_;
};







#endif //APPLICATION_HPP
